package edu.gatech.cc.liam.geometry.linear.programming;
 
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;

import edu.gatech.cc.liam.geometry.linear.Halfspace;
import edu.gatech.cc.liam.geometry.linear.NPoint;
import edu.gatech.cc.liam.geometry.linear.Polytope;
import edu.gatech.cc.liam.geometry.linear.convexhull.Preferences;


public class InteriorPoint extends NPoint {

	public InteriorPoint(double[] theValues) {
		super(theValues);
	}

	private static final long serialVersionUID = 1157234425427646224L;
	
	public double dist;

	public ArrayList<Halfspace> findNonAdjecentHypers(Collection<Halfspace> hypers) {
		ArrayList<Halfspace> nonAdjecentHypers = new ArrayList<Halfspace>();
		for(Halfspace h : hypers) {
			double distanceToPlane = h.pointDistanceFromHyperplane(this);
			if(distanceToPlane < -Preferences.TOLERANCE)
				nonAdjecentHypers.add(h);
		}
		return nonAdjecentHypers;
	}
	
	//the inequalities are all of the form  ... + offset <= 0
	// add an extra variable to find the distance from all inequalities
	private static LinearProb getPolytopeInteriorProbSol(Polytope theHalfSpaces) {
		int numNonBoundConstraints = theHalfSpaces.size();
		int numVariables = theHalfSpaces.getDimensions() + 1; // one extra for the offset included
		Polytope inequals = new Polytope();
		for(Halfspace h : theHalfSpaces) {
			double[] normal = Arrays.copyOf( h.normal, numVariables);
			normal[numVariables-1] = 1.0;
			inequals.add(new Halfspace(normal, h.offset));
		}
		NPoint obj = new NPoint(numVariables, 0.0);
		obj.values[numVariables-1] = 1.0;
		LinearProb lp = LinearProb.solveLP(inequals, obj);
		return lp;
	}
	
	public static InteriorPoint findHalfSpaceInteriorPoint(Polytope theHalfSpaces) {
		int numNonBoundConstraints = theHalfSpaces.size();
		int numVariables = theHalfSpaces.getDimensions() + 1; // one extra for the offset included

		LinearProb lp = getPolytopeInteriorProbSol(theHalfSpaces);

		double[] centerPoint = Arrays.copyOf(lp.getOptimalPoint(), numVariables - 1) ;
		InteriorPoint theInteriorPoint = new InteriorPoint(centerPoint);
		theInteriorPoint.dist = lp.getOptimalValue();
		
		// if polytope is infeasible, point may be outside with negative distance
		return theInteriorPoint;	
	}
	
	//the inequalities are all of the form  ... + offset <= 0
	// add an extra variable to find the distance from all inequalities
	private static LinearProb getPolytopeAugObjInteriorProbSol(Polytope theHalfSpaces, Polytope inequalsToCareAbout) {
		int numConstraints = theHalfSpaces.size() + inequalsToCareAbout.size();
		int numOrigVars = theHalfSpaces.getDimensions();
		int numSlackVars = inequalsToCareAbout.size();
		int numVariables = numOrigVars + numSlackVars;
		
		Polytope inequals = new Polytope();
		// make inequalities with slack variables on the halfspaces we care about
		int careNumber = 0;
		for(Halfspace h : theHalfSpaces) {
			double[] normal = Arrays.copyOf( h.normal, numVariables);
			if(inequalsToCareAbout.contains(h)) {
				normal[numOrigVars + careNumber] = 1.0;
				careNumber ++;
			}
			inequals.add(new Halfspace(normal, h.offset));
		}
		NPoint obj = new NPoint(numVariables, 0.0);
		
		for(int i = 0; i < numSlackVars; i++) {
			// maximize slack
			obj.values[numOrigVars + i] = 1.0;
			// make slack variables greater than 0 (so we only have feasible interior points)
			double[] normal = new double[numVariables];
			normal[numOrigVars + i] = -1.0;
			inequals.add(new Halfspace(normal, 0.0));
		}
		LinearProb lp = LinearProb.solveLP(inequals, obj);
		return lp;
	}
	
	public static InteriorPoint findHalfSpaceInteriorPointWithAugmentedObjective(Polytope theHalfSpaces, Polytope inequalsToCareAbout) {
		LinearProb lp = getPolytopeAugObjInteriorProbSol(theHalfSpaces, inequalsToCareAbout);
		double[] centerPoint = Arrays.copyOf(lp.getOptimalPoint(), theHalfSpaces.getDimensions());
		InteriorPoint theInteriorPoint = new InteriorPoint(centerPoint);
		theInteriorPoint.dist = lp.getOptimalValue();
		
		return theInteriorPoint;
	}
	
	public static void main(String[] args) {
		double tolerance = 0.000000001;
		//example
		 double[][] offCenteredCube =
			{{ 0, 0,-1,-0.9},
			 { 0,-1, 0,-0.0},
			 {-1, 0, 0,-0.5},
			 { 1, 0, 0,-0.5},
			 { 0, 1, 0,-1.0},
			 { 0, 0, 1,-0.1}};
		
		double[] centerPoint = findHalfSpaceInteriorPoint(new Polytope(offCenteredCube)).values;
		
		System.out.print("(");
		for(int k=0; k<centerPoint.length; k++) {
			System.out.print(", "+centerPoint[k]);
		}
		System.out.println(")");
		
	}

}
